/*
 * This file is part of the Chumby Computer Vision Project: www.EMGRobotics.com
 *
 * Copyright (c) 2010 individual Chumby Computer Vision contributors. 
 * See end of header for contributor list.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 *
 * In all cases you must keep this copyright notice intact, along with the
 * list of contributors at the end of this header.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 *
 * Contributors: 
 *			Eric Gregori ( www.EMGRobotics.com )
 */
//
//**********************************************************************************
//
//  ChumbyCV Algorithm - Color Blob Tracking
//
//  Compile this file, with a file containing your algorithm ( see example algorithm files )
//  gcc -L/usr/lib  -lXt -lX11 -lXaw ChumbyCV_X11.c ChumbyCV_ColorBlobTrack.c -o ChumbyCV_X11
//
//  Don't forget to chmod 777 ChumbyCV_X11 after building 
//
//
//  Command Line Parameters:
//
//
//**********************************************************************************
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>

//**********************************************************************************
// CallBack function
//**********************************************************************************
extern inline void SetPixel( uint32_t index, unsigned char r, unsigned char g, unsigned char b );

//**********************************************************************************
// Forward Declarations
//**********************************************************************************
int ChumbyCV_Algorithm_Init( void );

//**********************************************************************************
// Globals
//**********************************************************************************
uint32_t	G_Width, G_Height;
uint32_t	G_NumOfPixels;
uint32_t	G_Umax, G_Umin, G_Vmax, G_Vmin;
uint32_t	G_Urange, G_Vrange;
int		G_Measure;
unsigned char	G_Size;
uint32_t       *G_IntegralSpace;



static inline unsigned char limit( int data )
{
	if( data > 255 ) return( 255 );
	if( data < 0 ) return( 0 );
	return( data );
}

void SetMaxMin( uint32_t u, uint32_t v )
{
	G_Umax		= u + G_Urange/2; 
	G_Umin		= u - G_Urange/2; 
	G_Vmax		= v + G_Vrange/2;
	G_Vmin		= v - G_Vrange/2;
	
	if( G_Umax > 255 ) G_Umax = 255;
	if( G_Vmax > 255 ) G_Vmax = 255;
	if( G_Umin < 0   ) G_Umin = 0;
	if( G_Vmin < 0   ) G_Vmin = 0;
}

//**********************************************************************************
// Command Line Parameters:
//	argv[1] - video Device ( /dev/video0 )
//	argv[x] - Options
//			-txxx	Set Threshold ( default operation auto adjusts threshold	
//				Where xxx = decimal # from  1-254
//
// Return 0 for no error, !0 causes exit(1)
//**********************************************************************************
int ChumbyCV_Parameters( int argc, char *argv[], int width, int height )
{
	int	i, bad;
	int	r,g,b,u,v,set;

	G_Width  	= width;
	G_Height 	= height;
	G_NumOfPixels	= G_Width*G_Height;

	G_Umax		= 0; 
	G_Umin		= 255; 
	G_Vmax		= 0;
	G_Vmin		= 255;
	G_Urange	= 10;
	G_Vrange	= 10;
	r		= 0;
	g		= 0;
	b		= 0;
	G_Measure	= 1;
	G_Size		= 10;
	set		= 0;

	fprintf( stderr, "\nChumbyCV Color Blob Tracking Algorithm\n" );
	if( argc > 2 )
	{
		for( i=2, bad=1; i<argc; ++i )
		{
			if( argv[i][0] != '-' )
				break;
		
			switch( argv[i][1] )
			{
				case 't': // test mode
					G_Measure = -32000;
					bad = 0;
					break;

				case 'r': // red
					r = atoi( &argv[i][2] );
					bad = 0;
					set = 1;
					break;

				case 'g': // green
					g = atoi( &argv[i][2] );
					bad = 0;
					set = 1;
					break;

				case 'b': // blue
					b = atoi( &argv[i][2] );
					bad = 0;
					set = 1;
					break;

				case 'u': // u
					u = atoi( &argv[i][2] );
					bad = 0;
					set = 2;
					break;

				case 'v': // v
					v = atoi( &argv[i][2] );
					bad = 0;
					set = 2;
					break;

				case 'l': // U Range
					G_Urange = atoi( &argv[i][2] );
					bad = 0;
					break;

				case 'w': // V Range
					G_Vrange = atoi( &argv[i][2] );
					bad = 0;
					break;

				case 's': // S Size
					G_Size = atoi( &argv[i][2] );
					bad    = 0;
					break;

				default:
					break;
			}		
		}

		if( r > 255 || g > 255 || b > 255 )
			bad = 1;
		else if( set == 1 )
		{
			// calculate max and mins based on RGB inputs and range
			// Convert RGB to YUV
			// Y = ( (  66 * R + 129 * G +  25 * B + 128) >> 8) +  16
			// U = ( ( -38 * R -  74 * G + 112 * B + 128) >> 8) + 128
			// V = ( ( 112 * R -  94 * G -  18 * B + 128) >> 8) + 128
			u = limit(( ( -38 * r -  74 * g + 112 * b + 128) >> 8) + 128);
			v = limit(( ( 112 * r -  94 * g -  18 * b + 128) >> 8) + 128);

			SetMaxMin( u, v );
			fprintf( stderr, "Tracking U = %d, V = %d\n", u, v );
			G_Measure = 0;			
		}
		else if( set == 2 )
		{
			SetMaxMin( u, v );
			fprintf( stderr, "Tracking U = %d, V = %d\n", u, v );
			G_Measure = 0;			
		}

		if( bad )
		{
			fprintf( stderr, "\nUsage: %s V4L2_device [-txxx]\n", argv[0] );
			fprintf( stderr, "-txxx Set threshold to decimal xxx ( 0-255 )\n" );
			fprintf( stderr, "default is auto threshold adjust\n" );
			return 1;
		}
	} // if( argc > 2 )
	else
	{
		// No command line options specified
	}

	if( ChumbyCV_Algorithm_Init() > 0 )
		return 1;

	return 0; 
}

void ChumbyCV_ShutDown( void )
{
	free( G_IntegralSpace );
}

//**********************************************************************************
// Init Algorithm
//**********************************************************************************
int ChumbyCV_Algorithm_Init( void )
{
	// IntegralSpace only has to be as big as UV space
	G_IntegralSpace = (uint32_t *)malloc( (G_NumOfPixels/2)*sizeof(uint32_t) );
	if( G_IntegralSpace == NULL )	
		return 1;
	return 0;
}


//**********************************************************************************
//**********************************************************************************
static inline unsigned char CheckRange( unsigned char u, unsigned char v )
{
	if( u > G_Umin && u < G_Umax ) return( 1 );
	if( v > G_Vmin && v < G_Vmax ) return( 1 );
	return( 0 );
}


//**********************************************************************************
//**********************************************************************************
static inline void DisplayCrossHairs( uint32_t pos )
{
	uint32_t	i,k;

	// Display crosshairs at xpos, ypos
	for(i=(pos%G_Width); i<G_NumOfPixels; i+=G_Width )
	{
		SetPixel(i,0,255,0);
	}

	k = pos-(pos%G_Width);
	for( i=k; i<(k+G_Width); ++i )
	{
		SetPixel(i,0,255,0);
	}
}


//**********************************************************************************
//**********************************************************************************
static inline void DisplaySquare( void )
{
	uint32_t	i,k;

	for( i = (150+(110*G_Width)); i<(170+(130*G_Width)); i+=G_Width )
	{
		SetPixel(i,255,0,0);
		SetPixel(i+20,255,0,0);	
	}

	for( i = (150+(110*G_Width)); i<(170+(110*G_Width)); ++i )
	{
		SetPixel(i,255,0,0);
		SetPixel((i+(20*G_Width)),255,0,0);	
	}
}


//**********************************************************************************
// ChumbyCV_Algorithm - Motion Detection using frame subtraction
//
// This function is called by the ChumbyCV framework.  The char pointer CameraData
// points to a byte array of size (G_NumOfPixels*2).
// The data format is YUYV => 4 bytes = 2 pixels ( Y=pixel, U=color, V=color ) 
//
// Send result to stdio using format: <x,%x>
// Where "x" is the field name, and the value is after the ',' in hex
//**********************************************************************************
int ChumbyCV_Algorithm( unsigned char* CameraData )
{
	uint32_t CameraIndex;
	uint32_t DisplayIndex;
	uint32_t ColorIndex;
	unsigned char y1, y2, u, v, temp;
	uint32_t start, count, area1, area2, maxarea, pos;
	int	c,d,e;
	unsigned char 	r,g,b;

	if( G_Measure )
	{
		CameraIndex 	= 0;
		for(DisplayIndex=0; DisplayIndex<G_NumOfPixels; )// DisplayIndex -> used by SetPixel
		{
			// YUYV - 2 pixels for 1u, 1v
			y1 	= CameraData[CameraIndex++];			// Current frame from camera
			u  	= CameraData[CameraIndex++];
			y2 	= CameraData[CameraIndex++];
			v  	= CameraData[CameraIndex++];		

			if( 	((DisplayIndex%G_Width)>150) && 
				((DisplayIndex%G_Width)<170) &&
				((DisplayIndex/G_Width)>110) &&
				((DisplayIndex/G_Width)<130) )
			{ 
				// YUV to RGB
				c = y1 - 16;
				d = u - 128;
				e = v - 128;
				r = limit(( 298 * c           + 409 * e + 128) >> 8);
				g = limit(( 298 * c - 100 * d - 208 * e + 128) >> 8);
				b = limit(( 298 * c + 516 * d           + 128) >> 8);
				SetPixel(DisplayIndex++,r,g,b);

				c = y2 - 16;
				r = limit(( 298 * c           + 409 * e + 128) >> 8);
				g = limit(( 298 * c - 100 * d - 208 * e + 128) >> 8);
				b = limit(( 298 * c + 516 * d           + 128) >> 8);
				SetPixel(DisplayIndex++,r,g,b);

				if( DisplayIndex == (160+120*G_Width) )
				{
					SetMaxMin( u, v );
					fprintf( stderr, "U = %d, V = %d, R = %d, G = %d, B = %d\n", u, v, r,g,b );
				}
			}
			else
			{
				SetPixel(DisplayIndex++,y1,y1,y1);
				SetPixel(DisplayIndex++,y2,y2,y2);
			}

			DisplaySquare();
		}
		if( ++G_Measure > 100 )
		{
			G_Measure = 0;
			fprintf( stderr, "Umax = %d, Umin = %d, Vmax = %d, Vmin = %d\n", G_Umax, G_Umin, G_Vmax, G_Vmin );
		}
		return(0);
	} // G_Measure

	CameraIndex 	= 0;
	ColorIndex      = 0;
	memset( G_IntegralSpace, 0, G_NumOfPixels/2*sizeof(uint32_t) );
	for(DisplayIndex=0; DisplayIndex<G_NumOfPixels; ++DisplayIndex )// DisplayIndex -> used by SetPixel
	{
		// YUYV - 2 pixels for 1u, 1v
		y1 	= CameraData[CameraIndex++];			// Current frame from camera
		u  	= CameraData[CameraIndex++];
		y2 	= CameraData[CameraIndex++];
		v  	= CameraData[CameraIndex++];		

		// pixel 1
		if( CheckRange( u,v ) )
		{
			// pixel Difference detected
			temp = 1;					
			SetPixel(DisplayIndex,0,0,255);			// DisplayIndex=0 to G_NumOfPixels
		}
		else
		{
			// No pixel difference detected
			SetPixel(DisplayIndex,y1,y1,y1);		// DisplayIndex=0 to G_NumOfPixels
			temp = 0;
		}
	
		// Update Integral Space
		if( (DisplayIndex>(G_Width))&&(DisplayIndex%(G_Width)) ) // Ignore first row and column
		{
			// http://en.wikipedia.org/wiki/Summed_area_table
			G_IntegralSpace[ColorIndex] = temp +			//x,y
				G_IntegralSpace[ColorIndex-1]+			//x-1,y
				G_IntegralSpace[ColorIndex-(G_Width/2)]-	//x,y-1 
				G_IntegralSpace[ColorIndex-(G_Width/2)-1];	//x-1,y-1
		}
	
		++ColorIndex;
		++DisplayIndex;
		// pixel 2
		if( temp )
		{
			// pixel Difference detected
			SetPixel(DisplayIndex,0,0,255);			// DisplayIndex=0 to G_NumOfPixels
		}
		else
		{
			// No pixel difference detected
			SetPixel(DisplayIndex,y2,y2,y2);		// DisplayIndex=0 to G_NumOfPixels
		}
	} // for(DisplayIndex=0;

#if 0 // Display Integral Parameter Space for testing
	for(count=0; count<G_NumOfPixels/2; ++count )
	{
		area1 = G_IntegralSpace[count];
		if( area1 > 255 ) 
			y1 = 255;
		else
			y1 = area1;
		SetPixel(count*2,area1/0x100,y1,0);
	}

#endif

	// Look for max window of size ( G_Size ) in Integral Space
	start = (G_Size+(G_Size*G_Width/2));
	maxarea = G_Size*G_Size/10;
	pos     = 0;
	for(count=start; count<G_NumOfPixels/2; ++count )
	{
		if( (count%(G_Width/2)) < G_Size ) continue;
		// http://en.wikipedia.org/wiki/Summed_area_table
		area1 = (G_IntegralSpace[count] + 			// C
			G_IntegralSpace[count-start]); 			// A
		area2 = G_IntegralSpace[count-G_Size] +			// D
			G_IntegralSpace[count-(G_Size*(G_Width/2))];	// B
		if( (area1-area2) >= maxarea )
		{
			maxarea = (area1-area2);
			pos = count*2;
			//SetPixel(count*2,255,0,0);
		}
	}

	if( pos )
	{
		pos -= (G_Size/2);		// Set Position to center of mass
		pos -= (G_Width*G_Size/2);	// Set Position to center of mass
	}

	printf( "<x,%x><y,%x>\n", pos%G_Width, pos/G_Width ); 

	// Display position
	DisplayCrossHairs( pos );
	// end of Algorithm

// Simple sample code
// Display image from camera on screen
//
#if 0 // Simply display image on screen
	for(i=0, index=0; i<G_NumOfPixels; i++ )
	{
		// RGB24 output display - 0x00rrbbgg
		temp = CameraData[index+=2];
		SetPixel(i,temp,temp,temp);
	}
#endif
	return 0;
}

